#include <user.h>
#include <io.h>
#include <ts7200.h>
#include <curses.h>
#include <string.h>
#include <defs.h>

void NameServer();
void ClockServer();
void TerminalTxServer();
void TerminalRxServer();
void TrainTxServer();
void TrainRxServer();
void IdleTask();

// buffer sizes
#define CMD_BUF_SIZE 16

// command constants
#define TRAIN_SOLENOID_OFF  0x20
#define TRAIN_SW_STRAIGHT   0x21
#define TRAIN_SW_CURVED     0x22
#define TRAIN_REVERSE       0x0f
#define TRAIN_GO            0x60
#define TRAIN_STOP          0x61
#define TRAIN_SENSORS_BASE  0x80
#define TRAIN_SENSOR_BASE   0xc0

// other train set constants
#define NUM_SENSOR_MODULES  5

// message types
#define CLOCK_MSG       0
#define SENSOR_MSG      1
#define USER_INPUT_MSG  2

// interface message
typedef struct __interface_msg {
  int type;
  int c;
  int dc_ticks;
  char ss_status[10];
} interface_msg;

// sensor module info
typedef struct __sensor {
  int id;
  char b1;
  char b2;
} sensor;

void SensorPrint( sensor *ss ) {
  char group = 'A'+ss->id;
  unsigned short val = (ss->b1 << 8) | ss->b2;
  int bit = 16;
  while (!(val & 1)) {
    val >>= 1;
    bit--;
  }
  Printf( COM2, "%c%d\r\n", group, bit );
}

// train status info
typedef struct __train_status {
  char tr_speed[81];
  char sw_dir[256];
  sensor ss_mod[5];
  int ss_last;
} train_status;

void ClockMonitor() {
  interface_msg msg;
  msg.type = CLOCK_MSG;
  msg.dc_ticks = 0;

  int interface_tid;
  Receive(&interface_tid, NULL, 0);
  Reply(interface_tid, NULL, 0);
  // TODO: re-enable this
  for ( ; ; ) {
    Send(interface_tid, (char*)&msg, sizeof(interface_msg), NULL, 0);
    Delay(2);
    msg.dc_ticks = Time()/2;
  }
  Exit();
}

void UserInputMonitor() {
  interface_msg msg;
  msg.type = USER_INPUT_MSG;

  int interface_tid;
  Receive(&interface_tid, NULL, 0);
  Reply(interface_tid, NULL, 0);

  for ( ; ; ) {
    msg.c = Getc(COM2);
    Send(interface_tid, (char*)&msg, sizeof(interface_msg), NULL, 0);
  }
}

void SensorMonitor() {
  interface_msg msg;
  msg.type = SENSOR_MSG;

  int i, c;
  int interface_tid;
  Receive(&interface_tid, NULL, 0);
  Reply(interface_tid, NULL, 0);
  for ( ; ; ) {
    Putc( COM1, TRAIN_SENSORS_BASE + NUM_SENSOR_MODULES );
    for (i = 0; i < 10; i++) {
      c = Getc( COM1 );
      msg.ss_status[i] = c;
    }
    Send(interface_tid, (char*)&msg, sizeof(interface_msg), NULL, 0);
    Delay(10);
  }
}

int ParseInput(char *buf, char **cmd, char **a1, char **a2) {
  char *pos;
  *cmd = StringToken(buf, &pos, ' ');
  if (*cmd == NULL) return 0;
  *a1 = StringToken(NULL, &pos, ' ');
  if (*a1 == NULL) return 1;
  *a2 = StringToken(NULL, &pos, ' ');
  if (*a2 == NULL) return 2;
  return 3;
}

void Tr(train_status *sts, int train_number, int train_speed) {
  Putc( COM1, train_speed );
  Putc( COM1, train_number );
  sts->tr_speed[train_number] = train_speed;
}

void Rv(train_status *sts, int train_number) {
  Putc(COM1, 0);
  Putc(COM1, train_number);
  Delay(5);
  Putc(COM1, TRAIN_REVERSE);
  Putc(COM1, train_number);
  Delay(5);
  Putc(COM1, sts->tr_speed[train_number]);
  Putc(COM1, train_number);
}

void Sw(train_status *sts, int switch_number, char switch_direction) {
  if (switch_direction == 'S')
    Putc(COM1, TRAIN_SW_STRAIGHT);
  else
    Putc(COM1, TRAIN_SW_CURVED);
  Putc(COM1, switch_number);
  Delay(5);
  Putc(COM1, TRAIN_SOLENOID_OFF);
  sts->sw_dir[switch_number] = switch_direction;
}

void Wh(train_status *sts) {
  if (sts->ss_last == -1) {
    Printf( COM2, "no sensor triggered\r\n" );
    return;
  }
  SensorPrint( &sts->ss_mod[sts->ss_last] );
}

void St(train_status *sts, int switch_number) {
  Printf( COM2, "%c\r\n", sts->sw_dir[switch_number] );
}

void Stop(train_status *sts) {
  Putc( COM1, TRAIN_STOP );
}

void Go(train_status *sts) {
  Putc( COM1, TRAIN_GO );
}

void Clear(train_status *sts) {
  SetCursorXY(0, 2);
  ClearDown();
}

void Help(train_status *sts) {
  Printf( COM2, "tr train_number train_speed\r\n" );
  Printf( COM2, "\tset the given train to run at the given speed\r\n" );
  Printf( COM2, "rv train_number\r\n" );
  Printf( COM2, "\treverse the direction of the given train\r\n" );
  Printf( COM2, "sw switch_number switch_direction\r\n" );
  Printf( COM2, "\tset the given switch to the given position\r\n" );
  Printf( COM2, "wh\r\n" );
  Printf( COM2, "\tdisplay which sensor last saw the train\r\n" );
  Printf( COM2, "st switch_number\r\n" );
  Printf( COM2, "\tdisplay the position of the given switch\r\n" );
  Printf( COM2, "stop\r\n" );
  Printf( COM2, "\temergency stop operation\r\n" );
  Printf( COM2, "go\r\n" );
  Printf( COM2, "\tresume operation after a stop\r\n" );
  Printf( COM2, "clear\r\n" );
  Printf( COM2, "\tclear the terminal\r\n" );
}

int DoCommand(train_status *sts, char *cmd_buf) {
  char *cmd, *a1, *a2;
  int argc, v1, v2;
  argc = ParseInput(cmd_buf, &cmd, &a1, &a2);
  if (StringCompare(cmd, "tr", 2)) {
    if (argc != 3) {
      Printf( COM2, "Usage: tr train_number train_speed\r\n" );
      return 1;
    }
    v1 = Atoi(a1); v2 = Atoi(a2);
    if (v1 < 1 || v1 > 80) {
      Printf( COM2, "Error: invalid train number %d!\r\n", v1 );
      return 1;
    }
    if (v2 < 0 || v2 > 14) {
      Printf( COM2, "Error: invalid train speed %d!\r\n", v2 );
      return 1;
    }
    Tr(sts, v1, v2);
  } else if (StringCompare(cmd, "rv", 2)) {
    if (argc != 2) {
      Printf( COM2, "got %d args, expected 2\r\n", argc );
      Printf( COM2, "Usage: rv train_number\r\n" );
      return 1;
    }
    v1 = Atoi(a1);
    if (v1 < 1 || v1 > 80) {
      Printf( COM2, "Error: invalid train number %d!\r\n", v1 );
      return 1;
    }
    Rv(sts, v1);
  } else if (StringCompare(cmd, "sw", 2)) {
    if (argc != 3) {
      Printf( COM2, "Usage: sw switch_number switch_direction\r\n" );
      return 1;
    }
    v1 = Atoi(a1);
    if (v1 < 0 || v1 > 255) {
      Printf( COM2, "Error: invalid switch number %d!\r\n", v1 );
      return 1;
    }
    if (a2[0] != 'S' && a2[0] != 'C') {
      Printf ( COM2, "Error: invalid switch direction %c!\r\n", a2[0] );
      return 1;
    }
    Sw(sts, v1, a2[0]);
  } else if (StringCompare(cmd, "wh", 2)) {
    if (argc != 1) {
      Printf( COM2, "Usage: wh\r\n" );
      return 1;
    }
    Wh(sts);
  } else if (StringCompare(cmd, "stop", 4)) {
    Stop(sts);
  } else if (StringCompare(cmd, "st", 2)) {
    if (argc != 2) {
      Printf( COM2, "Usage: st switch_number\r\n" );
      return 1;
    }
    v1 = Atoi(a1);
    if (v1 < 0 || v1 > 255) {
      Printf( COM2, "Error: invalid switch number %d!\r\n", v1 );
      return 1;
    }
    St(sts, v1);
  } else if (StringCompare(cmd, "go", 2)) {
    Go(sts);
  } else if (StringCompare(cmd, "help", 4)) {
    Help(sts);
  } else if (StringCompare(cmd, "clear", 5)) {
    Clear(sts);
  } else if (StringCompare(cmd, "q", 1)) {
    Printf(COM2, "Bye!\r\n");
    FingerOfDeath();
  } else {
    Printf(COM2, "%s: command not found\r\n", cmd);
    return 1;
  }
  return SUCCESS;
}

void UpdateDisplayClock(int dc_ticks) {
  SetCursorXY(0, 1);
  ClearLine();
  Printf(COM2, "%d%d:%d%d.%d", dc_ticks/6000, (dc_ticks/600)%10,
      (dc_ticks/100)%6, (dc_ticks/10)%10, dc_ticks%10);
}

void InterfaceInit( train_status *sts ) {
  int i;
  
  // initialize screen
  ClearScreen();
  SetScreenColor(FG_BASE + WHITE_OFFSET, BG_BASE + BLACK_OFFSET);
  SetCursorXY(0, 2);
  
  // initialize track state
  Printf( COM2, "stopping trains..." );
  for (i = 1; i <= 80; i++) {
    Tr(sts, i, 0);
  }
  Printf( COM2, "done.\r\n" );

  // initialize switch positions FOR EXISTING SWITCHES ONLY to curved
  Printf( COM2, "curving switches..." );
  for (i = 1; i <= 156; i++) {
    if (i == 19) i = 153;   // skip non-existent switches
    Sw(sts, i, 'C');
  }
  Printf( COM2, "done.\r\n" );

  // initialize sensor stuff
  sts->ss_last = -1;

  // initialize shell
  Printf(COM2, "> ");
  SaveCursor();
}

void Interface() {
  int i;

  // initialize command-line buffers
  char cmd_buf[CMD_BUF_SIZE+1];
  cmd_buf[0] = '\0';
  int cmd_ptr = 0;

  // initialize space for messages
  interface_msg msg;
  
  // initialize train status
  train_status sts;

  InterfaceInit( &sts );
  
  // TODO: enable these once concurrent interrupts work
  //int clock_tid = -1;
  int sensor_tid = -1;
  int clock_tid = Create(6, &ClockMonitor);
  //int sensor_tid = Create(4, &SensorMonitor);
  int user_input_tid = Create(5, &UserInputMonitor);
  Send(clock_tid, NULL, 0, NULL, 0);
  //Send(sensor_tid, NULL, 0, NULL, 0);
  Send(user_input_tid, NULL, 0, NULL, 0);

  int sender_tid;
  for ( ; ; ) {
    Receive(&sender_tid, (char*)&msg, sizeof(interface_msg));
    if (sender_tid == clock_tid) {
      UpdateDisplayClock(msg.dc_ticks);
      Reply(clock_tid, NULL, 0);
    } else if (sender_tid == sensor_tid) {
      sts.ss_last = -1;
      for (i = 0; i < 5; i++) {
        sts.ss_mod[i].id = i;
        sts.ss_mod[i].b1 = msg.ss_status[2*i];
        sts.ss_mod[i].b2 = msg.ss_status[2*i+1];
        if (sts.ss_mod[i].b1 || sts.ss_mod[i].b2) {
          sts.ss_last = i;
        }
      }
      Reply(sensor_tid, NULL, 0);
    } else if (sender_tid == user_input_tid) {
      LoadCursor();
      if (msg.c == '\r') {
        Printf( COM2, "\r\n" );
        cmd_buf[cmd_ptr] = '\0';
        DoCommand(&sts, cmd_buf);
        Printf( COM2, "> " );
        cmd_ptr = 0;
      } else if (msg.c == '\x7f' && cmd_ptr > 0) {
        MoveCursorXY(-1, 0);
        Printf(COM2, "%c", ' ');
        MoveCursorXY(-1, 0);
        cmd_ptr--;
      } else if (IsAlnum(msg.c) && cmd_ptr < CMD_BUF_SIZE) {
        Printf(COM2, "%c", msg.c);
        cmd_buf[cmd_ptr++] = msg.c;
      }
      SaveCursor();
      Reply(user_input_tid, NULL, 0);
    }
  }
  Exit();
}

void UserInit() {
  Create( 8, &NameServer );
  Create( 8, &ClockServer );
  Create( 8, &TerminalTxServer );
  Create( 8, &TerminalRxServer );
  Create( 8, &TrainTxServer );
  Create( 8, &TrainRxServer );
  Create( 0, &IdleTask );
  Create( 5, &Interface );
  Exit();
}
